#pragma once

#include "stdafx.h"
#include "tnt.h"

/*#define TIME_STEP .05f
#define FRICTION 0.0f
#define POSITION 0.050f
#define VELOCITY 0.0025f
#define ACCELERATION 0.005f*/

#define TIME_STEP .05f
#define FRICTION 0.0f
#define POSITION 0.5f
#define VELOCITY 0.5f
#define ACCELERATION 1.0f

class KalmanFilter
{
public:
	KalmanFilter(Vector2 initial_position);
	~KalmanFilter(void);

	void UpdateMatricies();
	Vector2 Predict(float deltaTime);
	Vector2 GetCurrentEstimate();
	float Sigma_X();
	float Sigma_Y();
	float Rho();

	void ResetEx();

	virtual bool ObjectIsOnBoard() {
		return true;
	}
	virtual Vector2 GetObservedPosition() {
		return Vector2();
	};

private:
	TNT::Array2D<float> CalcCommon();

	TNT::Array2D<float> M;
	TNT::Array2D<float> M1;
	TNT::Array2D<float> K;
	TNT::Array2D<float> Ex;
	TNT::Array2D<float> Ez;
	TNT::Array2D<float> Et;
	TNT::Array2D<float> H;
	TNT::Array2D<float> F;
	TNT::Array2D<float> Ht;
	TNT::Array2D<float> Ft;
	TNT::Array2D<float> I;
};

